/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/**
 * @file NDS03_ContiunousRanging_Example.c
 * @author tongsheng.tang
 * @brief 连续测量示例
 * @version 2.x.x
 * @date 2024-04
 *
 * @copyright Copyright (c) 2024, Nephotonics Information Technology (Hefei) Co., Ltd.
 *
 */

#include <stdio.h>
#include "nds03_platform.h"
#include "nds03_comm.h"
#include "nds03_dev.h"
#include "nds03_data.h"
#include "nds03_calib.h"
#include "nds03_def.h"

#include "zino.h"
#include "drv_common.h"
#include <rtconfig.h>
#include <rtthread.h>

#define DBG_TAG "ND03/usr"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

/** 模组设备 */
NDS03_Dev_t g_nds03_device = {
    .platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR,
    .platform.xshut_pin = GET_PIN(B, 6),
};

/**
 * @brief 单次测量接口
 *
 * @param pNxDevice 模组设备
 */
void SG_RangingTest(NDS03_Dev_t *pNxDevice)
{
    /* 获取测量数据 */
    NDS03_GetContinuousRangingData(pNxDevice);
    NX_PRINTF("depth1:%d mm, confi:%d, count:%d\r\n", pNxDevice->ranging_data[0].depth, pNxDevice->ranging_data[0].confi, pNxDevice->ranging_data[0].count);
    NX_PRINTF("depth2:%d mm, confi:%d, count:%d\r\n", pNxDevice->ranging_data[1].depth, pNxDevice->ranging_data[1].confi, pNxDevice->ranging_data[1].count);
    NX_PRINTF("depth3:%d mm, confi:%d, count:%d\r\n", pNxDevice->ranging_data[2].depth, pNxDevice->ranging_data[2].confi, pNxDevice->ranging_data[2].count);
    NX_PRINTF("depth4:%d mm, confi:%d, count:%d\r\n", pNxDevice->ranging_data[3].depth, pNxDevice->ranging_data[3].confi, pNxDevice->ranging_data[3].count);
}

/**
 * @brief NDS03连续测量主函数示例
 *
 * @return int32_t
 */
int32_t nds03_main(void)
{
    NDS03_Dev_t *pNxDevice = &g_nds03_device;
    /* 循环测距次数为100次 */
    // int32_t cnt = 100;

    /* 初始化平台，包含I2C初始化 */
    if (NDS03_ERROR_NONE != nds03_platform_init(&pNxDevice->platform, NULL))
    {
        NX_PRINTF("nds03_platform init error\n");
        return NDS03_ERROR_API;
    }

    /* 初始化设备变量的IIC地址 */
    pNxDevice->platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR;

    /* 循环等待设备启动, 若模组或者IIC读写函数有问题则会报错 */
    if (NDS03_ERROR_NONE != NDS03_WaitDeviceBootUp(pNxDevice))
    {
        NX_PRINTF("NDS03_WaitDeviceBootUp error\r\n");
        return NDS03_ERROR_BOOT;
    }

    /* 初始化模组设备 */
    if (NDS03_ERROR_NONE != NDS03_InitDevice(pNxDevice))
    {
        NX_PRINTF("NDS03_InitDevice error!!\r\n");
        return NDS03_ERROR_INIT;
    }

    /* 连续模式开启测量 */
    NDS03_StartContinuousMeasurement(pNxDevice);

    /* 循环测量 */
    while (1)
    {
        SG_RangingTest(pNxDevice);
        // rt_thread_mdelay(10);
        // LOG_D("cnt = %d", cnt);
    }
    /* 连续模式关闭测量 */
    NDS03_StopContinuousMeasurement(pNxDevice);

    /* 释放平台 */
    nds03_platform_uninit(&pNxDevice->platform, NULL);

    return 0;
}
int nds03_continuous_ranging_init(NDS03_Dev_t *pNxDevice)
{
    /* 初始化平台，包含I2C初始化 */
    if (NDS03_ERROR_NONE != nds03_platform_init(&pNxDevice->platform, NULL))
    {
        NX_PRINTF("nds03_platform init error\n");
        return NDS03_ERROR_API;
    }

    /* 初始化设备变量的IIC地址 */
    pNxDevice->platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR;

    /* 循环等待设备启动, 若模组或者IIC读写函数有问题则会报错 */
    if (NDS03_ERROR_NONE != NDS03_WaitDeviceBootUp(pNxDevice))
    {
        NX_PRINTF("NDS03_WaitDeviceBootUp error\r\n");
        return NDS03_ERROR_BOOT;
    }

    /* 初始化模组设备 */
    if (NDS03_ERROR_NONE != NDS03_InitDevice(pNxDevice))
    {
        NX_PRINTF("NDS03_InitDevice error!!\r\n");
        return NDS03_ERROR_INIT;
    }
    uint32_t frame_time = 0;
    uint32_t pulse_num = 0;
    uint8_t target_num = 0;
    NDS03_GetPulseNum(pNxDevice, &pulse_num);//390000
    NDS03_GetTargetNum(pNxDevice, &target_num);//4
    NDS03_GetFrameTime(pNxDevice, &frame_time);//33333

    NDS03_SetFrameTime(pNxDevice, 20000);
    // NDS03_SetPulseNum(pNxDevice, 1000);
    NDS03_SetTargetNum(pNxDevice, 1);

    LOG_D("frame_time = %d, pulse_num = %d, target_num = %d\n", frame_time, pulse_num, target_num);

    /* 连续模式开启测量 */
    NDS03_StartContinuousMeasurement(pNxDevice);
    return 0;
}
int nds03_stop_measurement(NDS03_Dev_t *pNxDevice)
{
    /* 连续模式关闭测量 */
    NDS03_StopContinuousMeasurement(pNxDevice);

    /* 释放平台 */
    nds03_platform_uninit(&pNxDevice->platform, NULL);
    return 0;
}
int nds03_power_down(NDS03_Dev_t *pNxDevice)
{
    /* 关闭电源 */
    NDS03_SetXShutPinLevel(pNxDevice, 0);
    return 0;
}
// int nd03_task_init()
// {

//     rt_thread_t nds03_thread = rt_thread_create("fc_error_handle", nds03_main, RT_NULL, 512, 10, 10);
//     if (nds03_thread != RT_NULL)
//     {
//         rt_thread_startup(nds03_thread);
//         LOG_I("nds03_thread create success.");
//     }
//     else
//     {
//         LOG_E("nds03_thread create failed.");
//     }
//     return 0;
// }
// ZINO_APP_EXPORT(nd03_task_init);

#include "sensor.h"
static rt_err_t nds03_control(struct rt_sensor_device *sensor, int cmd, void *args)
{
    rt_err_t result = RT_EOK;
    NDS03_Dev_t *pNxDevice = sensor->parent.user_data;
    switch (cmd)
    {
    case RT_SENSOR_CTRL_GET_ID:
        if (i2c_device_try("i2c2", NDS03_DEFAULT_SLAVE_ADDR) == 1)
        {
            *(rt_uint8_t *)args = NDS03_DEFAULT_SLAVE_ADDR;
        }
        break;
    case RT_SENSOR_CTRL_SET_POWER:
    {
        rt_uint32_t pctrl = (rt_uint32_t)args;
        if (pctrl == RT_SENSOR_POWER_DOWN)
        {
            nds03_power_down(pNxDevice);
            LOG_D("NDS03 power down");
        }
        else
        {
            result = nds03_continuous_ranging_init(pNxDevice);
            LOG_D("NDS03 power up");
        }
    }
    break;
    default:
        break;
    }
    return result;
}
static rt_size_t nds03_fect_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len)
{
    struct rt_sensor_data *data = (struct rt_sensor_data *)buf;
    NDS03_Dev_t *pNxDevice = sensor->parent.user_data;
    if (sensor->info.type == RT_SENSOR_CLASS_TOF)
    {
        /* 获取测量数据 */
        NDS03_GetContinuousRangingData(pNxDevice);
        data->timestamp = rt_sensor_get_ts();
        data->type = RT_SENSOR_CLASS_TOF;
        data->data.tof.confi = pNxDevice->ranging_data[0].confi;
        data->data.tof.count = pNxDevice->ranging_data[0].count;
        data->data.tof.crate = pNxDevice->ranging_data[0].crate;
        data->data.tof.depth = pNxDevice->ranging_data[0].depth;

    // NX_PRINTF("depth1:%4d mm, confi:%4d, count:%4d\r\n", pNxDevice->ranging_data[0].depth, pNxDevice->ranging_data[0].confi, pNxDevice->ranging_data[0].count);
    // NX_PRINTF("depth2:%4d mm, confi:%4d, count:%4d \t", pNxDevice->ranging_data[1].depth, pNxDevice->ranging_data[1].confi, pNxDevice->ranging_data[1].count);
    // NX_PRINTF("depth3:%4d mm, confi:%4d, count:%4d \t", pNxDevice->ranging_data[2].depth, pNxDevice->ranging_data[2].confi, pNxDevice->ranging_data[2].count);
    // NX_PRINTF("depth4:%4d mm, confi:%4d, count:%4d \t", pNxDevice->ranging_data[3].depth, pNxDevice->ranging_data[3].confi, pNxDevice->ranging_data[3].count);

    }
    return 1;
}

static struct rt_sensor_ops nds03_ops =
{
    nds03_fect_data,
    nds03_control,
};

int init_tof_sensor(void)
{
    rt_int8_t result;
    rt_int8_t tryI2c=0;
    rt_sensor_t sensor = RT_NULL;
    static NDS03_Dev_t g_nds03_device = {
        .platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR,
        .platform.xshut_pin = GET_PIN(B, 6),
    };
    while(1)
    {
        NDS03_Wakeup(&g_nds03_device);
        rt_thread_mdelay(1);
        if (i2c_device_try("i2c2", NDS03_DEFAULT_SLAVE_ADDR) == 1)
        {
            result = nds03_continuous_ranging_init(&g_nds03_device);
            if(result == NDS03_ERROR_NONE)
            {
                nds03_power_down(&g_nds03_device);
                break;
            }
        }
        // rt_thread_mdelay(1);
        if( tryI2c++ > 3)
        {
            LOG_E("can't find NDS03 sensor");
            return -1;
        }
    }
    sensor = rt_calloc(1, sizeof(struct rt_sensor_device));
    if(RT_NULL == sensor)
    {
        LOG_E("fail to rt_calloc");
        return -1;
    }

    sensor->info.type = RT_SENSOR_CLASS_TOF;
    sensor->info.vendor = RT_SENSOR_VENDOR_MERRILLCHIP;
    sensor->info.model = "NDS03";
    sensor->info.unit = RT_SENSOR_UNIT_MM;
    sensor->info.intf_type = RT_SENSOR_INTF_I2C;
    sensor->info.range_max = 4500;
    sensor->info.range_min = 0;
    sensor->info.period_min= 33;
    sensor->info.fifo_max = 0;

    sensor->ops = &nds03_ops;


    result = rt_hw_sensor_register(sensor, "NDS03", RT_DEVICE_FLAG_RDWR, &g_nds03_device);

    return result;
}
ZINO_DEVICE_EXPORT(init_tof_sensor);
